/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.grt192.mechanism.breakaway;

import com.grt192.actuator.GRTJaguar;
import com.grt192.core.Command;
import com.grt192.core.Mechanism;
import com.grt192.event.GlobalEvent;
import com.grt192.event.GlobalListener;

import com.grt192.event.component.PotentiometerEvent;
import com.grt192.event.component.PotentiometerListener;
import com.grt192.sensor.GRTPotentiometer;
import edu.wpi.first.wpilibj.templates.MainRobot;

/**
 *
 * @author Vikram Sunder
 */
public class Roller extends Mechanism implements GlobalListener {

	public static final double GRAB = 1.0;
    public static final double PUSH = -1.0;
    public static final double STOP = 0;
    public static final double CENTER_THRESHOLD = 20;
    public static final double SIDE_THRESHOLD = 80;
    private boolean rollerRunning = false;
    private double speed;

    public Roller(GRTJaguar rollerLeft, GRTJaguar rollerRight) {
        //starts motors
        rollerRight.start();
        rollerLeft.start();

        //adds Actuators
        addActuator("RightRollerMotor", rollerRight);
        addActuator("LeftRollerMotor", rollerLeft);
        speed = 0;
        MainRobot.getInstance().addGlobalListener(this);
    }

    public void setLeftSpeed(double input) {
        getActuator("LeftRollerMotor").enqueueCommand(new Command(-input));
    }

    public void setRightSpeed(double input) {
        getActuator("RightRollerMotor").enqueueCommand(new Command(input));
    }

    public void grab() {
        leftGrab();
        rightGrab();
        speed = GRAB;
        rollerRunning = true;
    }

    public void grab(double speed) {
        setLeftSpeed(speed);
        setRightSpeed(speed);
        this.speed = speed;
        rollerRunning = true;
    }

    public void stop() {
        stopLeftRoller();
        stopRightRoller();
        speed = 0;
        rollerRunning = false;
    }

    public void push() {
        leftPush();
        rightPush();
        speed = PUSH;
        rollerRunning = true;
    }

    public void leftGrab() {
        setLeftSpeed(GRAB);
    }

    public void rightGrab() {
        setRightSpeed(GRAB);
    }

    public void leftPush() {
        setLeftSpeed(PUSH);
    }

    public void rightPush() {
        setRightSpeed(PUSH);
    }

    public void stopRightRoller() {
        getActuator("RightRollerMotor").enqueueCommand(new Command(speed/2.0, 200));
        setRightSpeed(STOP);
    }

    public void stopLeftRoller() {
        getActuator("LeftRollerMotor").enqueueCommand(new Command(speed/2.0, 200));
        setLeftSpeed(STOP);
    }

    public boolean isRunning() {
        return rollerRunning;
    }

    public void globalChanged(GlobalEvent e) {
        if (e.getKey().equals("Kicking")) {
            if (((Boolean) e.getGlobals().get("Kicking")).booleanValue()) {
                stop();
                System.out.println("Roller disabled");
            }
        }

    }

}
